using BepuUtilities;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
using static BepuUtilities.GatherScatter;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.Constraints
{
    /// <summary>
    /// 铰链的角度组件。约束两个实体的角度自由度,使它们只能围绕铰链的轴相对于彼此旋转。
    /// </summary>
    public struct AngularHinge : ITwoBodyConstraintDescription<AngularHinge>
    {
        /// <summary>
        /// A的局部空间中的铰链轴线。
        /// </summary>
        public Vector3 LocalHingeAxisA;
        /// <summary>
        /// B的局部空间中的铰链轴线。
        /// </summary>
        public Vector3 LocalHingeAxisB;
        /// <summary>
        /// 弹簧频率和阻尼参数。
        /// </summary>
        public SpringSettings SpringSettings;

        public int ConstraintTypeId
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                return AngularHingeTypeProcessor.BatchTypeId;
            }
        }

        public Type TypeProcessorType => typeof(AngularHingeTypeProcessor);

        public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
        {
            ConstraintChecker.AssertUnitLength(LocalHingeAxisA, nameof(AngularHinge), nameof(LocalHingeAxisA));
            ConstraintChecker.AssertUnitLength(LocalHingeAxisB, nameof(AngularHinge), nameof(LocalHingeAxisB));
            ConstraintChecker.AssertValid(SpringSettings, nameof(AngularHinge));
            Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type.");
            ref var target = ref GetOffsetInstance(ref Buffer<AngularHingePrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
            Vector3Wide.WriteFirst(LocalHingeAxisA, ref target.LocalHingeAxisA);
            Vector3Wide.WriteFirst(LocalHingeAxisB, ref target.LocalHingeAxisB);
            GetFirst(ref target.SpringSettings.AngularFrequency) = SpringSettings.AngularFrequency;
            GetFirst(ref target.SpringSettings.TwiceDampingRatio) = SpringSettings.TwiceDampingRatio;
        }

        public void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out AngularHinge description)
        {
            Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type.");
            ref var source = ref GetOffsetInstance(ref Buffer<AngularHingePrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
            Vector3Wide.ReadFirst(source.LocalHingeAxisA, out description.LocalHingeAxisA);
            Vector3Wide.ReadFirst(source.LocalHingeAxisB, out description.LocalHingeAxisB);
            description.SpringSettings.AngularFrequency = GetFirst(ref source.SpringSettings.AngularFrequency);
            description.SpringSettings.TwiceDampingRatio = GetFirst(ref source.SpringSettings.TwiceDampingRatio);
        }
    }

    public struct AngularHingePrestepData
    {
        public Vector3Wide LocalHingeAxisA;
        public Vector3Wide LocalHingeAxisB;
        public SpringSettingsWide SpringSettings;
    }

    public struct AngularHingeProjection
    {
        // JacobianB=-JacobianA,所以不需要显式存储它。
        public Matrix2x3Wide VelocityToImpulseA;
        public Vector2Wide BiasImpulse;
        public Vector<float> SoftnessImpulseScale;
        public Matrix2x3Wide ImpulseToVelocityA;
        public Matrix2x3Wide NegatedImpulseToVelocityB;
    }


    public struct AngularHingeFunctions : IConstraintFunctions<AngularHingePrestepData, AngularHingeProjection, Vector2Wide>
    {
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void GetErrorAngles(in Vector3Wide hingeAxisA, in Vector3Wide hingeAxisB, in Matrix2x3Wide jacobianA, out Vector2Wide errorAngles)
        {
            // 计算位置误差和偏置速度。
            // 现在我们只有一点小小的烦恼,我们的误差函数包含反三角。
            // 我们将只使用：
            // ATAN(dot(hingeAxisBOnPlane X,hingeAxisA),dot(hingeAxisBOnPlane X,constraintAxisAY))=
            // sign(dot(hingeAxisBOnPlane X,constraintAxisAY))*acos(dot(hingeAxisA,hingeAxisBOnPlane X/||hingeAxisBOnPlane X||))
            // TODO：如果您愿意接受更多错误,您可以使用一些可选的近似方法来加快速度。
            // V1放弃了逆触发器,只使用了等价的点积,它.有问题,但是,嘿,它很便宜。
            Vector3Wide.Dot(hingeAxisB, jacobianA.X, out var hingeAxisBDotX);
            Vector3Wide.Dot(hingeAxisB, jacobianA.Y, out var hingeAxisBDotY);
            Vector3Wide.Scale(jacobianA.X, hingeAxisBDotX, out var toRemoveX);
            Vector3Wide.Scale(jacobianA.Y, hingeAxisBDotY, out var toRemoveY);
            Vector3Wide.Subtract(hingeAxisB, toRemoveX, out var hingeAxisBOnPlaneX);
            Vector3Wide.Subtract(hingeAxisB, toRemoveY, out var hingeAxisBOnPlaneY);
            Vector3Wide.Length(hingeAxisBOnPlaneX, out var xLength);
            Vector3Wide.Length(hingeAxisBOnPlaneY, out var yLength);
            var scaleX = Vector<float>.One / xLength;
            var scaleY = Vector<float>.One / yLength;
            Vector3Wide.Scale(hingeAxisBOnPlaneX, scaleX, out hingeAxisBOnPlaneX);
            Vector3Wide.Scale(hingeAxisBOnPlaneY, scaleY, out hingeAxisBOnPlaneY);
            // 如果轴与平面的法线平行,只需任意拾取0角度即可。
            var epsilon = new Vector<float>(1e-7f);
            var useFallbackX = Vector.LessThan(xLength, epsilon);
            var useFallbackY = Vector.LessThan(yLength, epsilon);
            Vector3Wide.ConditionalSelect(useFallbackX, hingeAxisA, hingeAxisBOnPlaneX, out hingeAxisBOnPlaneX);
            Vector3Wide.ConditionalSelect(useFallbackY, hingeAxisA, hingeAxisBOnPlaneY, out hingeAxisBOnPlaneY);

            Vector3Wide.Dot(hingeAxisBOnPlaneX, hingeAxisA, out var hbxha);
            Vector3Wide.Dot(hingeAxisBOnPlaneY, hingeAxisA, out var hbyha);
            // 我们可能会得到类似于(1-x)*pi/2的ACOS近似值,但我们只需要多做一点工作：
            MathHelper.ApproximateAcos(hbxha, out errorAngles.X);
            MathHelper.ApproximateAcos(hbyha, out errorAngles.Y);
            Vector3Wide.Dot(hingeAxisBOnPlaneX, jacobianA.Y, out var hbxay);
            Vector3Wide.Dot(hingeAxisBOnPlaneY, jacobianA.X, out var hbyax);
            errorAngles.X = Vector.ConditionalSelect(Vector.LessThan(hbxay, Vector<float>.Zero), errorAngles.X, -errorAngles.X);
            errorAngles.Y = Vector.ConditionalSelect(Vector.LessThan(hbyax, Vector<float>.Zero), -errorAngles.Y, errorAngles.Y);
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB,
            ref AngularHingePrestepData prestep, out AngularHingeProjection projection)
        {
            bodies.GatherOrientation(ref bodyReferences, count, out var orientationA, out var orientationB);

            // 请注意,我们首先在局部空间中构建切线以避免不一致。
            Helpers.BuildOrthnormalBasis(prestep.LocalHingeAxisA, out var localAX, out var localAY);
            Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA);
            Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalHingeAxisA, orientationMatrixA, out var hingeAxisA);
            Matrix2x3Wide jacobianA;
            Matrix3x3Wide.TransformWithoutOverlap(localAX, orientationMatrixA, out jacobianA.X);
            Matrix3x3Wide.TransformWithoutOverlap(localAY, orientationMatrixA, out jacobianA.Y);
            QuaternionWide.TransformWithoutOverlap(prestep.LocalHingeAxisB, orientationB, out var hingeAxisB);

            // 我们将hingeAxisB投影到由A的X轴和Y轴定义的平面上,并将它们视为相对于A的速度为常数。
            // 这只手挥走了与A的轴也有速度相关的一些复杂性,但它最终还是很好地解决了问题。
            // hingeAxisBOnPlane X=hingeAxisB-dot(constraintAxisX,hingeAxisB)*constraintAxisX
            // hingeAxisBOnPlane Y=hingeAxisB-dot(constraintAxisY,hingeAxisB)*constraintAxisY
            // 请注意,我们在这里实际上使用了逆三角。这在很大程度上是为了公式,导数最终会很好地崩溃。
            // C=[atan(dot(hingeAxisBOnPlane X,hingeAxisA),dot(hingeAxisBOnPlane X,constraintAxisAY))]=[0]
            // [ATAN(dot(hingeAxisBOnPlane Y,hingeAxisA),dot(hingeAxisBOnPlane Y,constraintAxisAX))][0]
            // 关注hingeAxisOnPlane X Jacobian：
            // C‘=(dot(hingeAxisBOnPlane X,hingeAxisA)*d/dt(dot(hingeAxisBOnPlane X,constraintAxisAY)-
            // D/dt(dot(hingeAxisBOnPlane X,hingeAxisA))*dot(hingeAxisBOnPlane X,constraintAxisAY))*denom
            // 其中denom=1f/(dot(hingeAxisBOnPlane X,hingeAxisA)^2+dot(hingeAxisBOnPlane X,constraintAxisAY)^2)
            // C‘=(dot(hingeAxisBOnPlane X,hingeAxisA)*(dot(d/dt(HingeAxisBOnPlane X),constraintAxisAY)+dot(hingeAxisBOnPlane X,d/dt(ConstraintAxisAY)-
            // (dot(d/dt(HingeAxisBOnPlane X),hingeAxisA)+dot(hingeAxisBOnPlane X,d/dt(HingeAxisA)*dot(hingeAxisBOnPlane X,constraintAxisAY))*denom
            // C‘=(dot(hingeAxisBOnPlane X,hingeAxisA)*(dot(wb x hingeAxisBOnPlane X,constraintAxisAY)+dot(hingeAxisBOnPlane X,WA x constraintAxisAY))-
            // (dot(wb x hingeAxisBOnPlane X,hingeAxisA)+dot(hingeAxisBOnPlane X,WA x hingeAxisA))*dot(hingeAxisBOnPlane X,constraintAxisAY))*denom
            // C‘=(dot(hingeAxisBOnPlane X,hingeAxisA)*(dot(hingeAxisBOnPlane X x constraintAxisAY,wb)+dot(WA,constraintAxisAY x hingeAxisBOnPlane X)-
            // (dot(hingeAxisBOnPlane X x hingeAxisA,WB)+dot(WA,hingeAxisA x hingeAxisBOnPlane X))*dot(hingeAxisBOnPlane X,constraintAxisAY))*denom
            // C‘=((dot(dot(hingeAxisBOnPlane X,hingeAxisA)*(HingeAxisBOnPlane X X ConstraintAxisAY),wb)+dot(WA,dot(hingeAxisBOnPlane X,hingeAxisA)*(ConstraintAxisAY X HingeAxisBOnPlane X)-
            // (dot((HingeAxisBOnPlane X X HingeAxisA)*dot(hingeAxisBOnPlane X,constraintAxisAY),wb)+dot(WA,(HingeAxisA X HingeAxisBOnPlane X)*dot(hingeAxisBOnPlane X,constraintAxisAY)*denom

            // C‘=((dot(dot(hingeAxisBOnPlane X,hingeAxisA)*(HingeAxisBOnPlane X X ConstraintAxisAY)-(HingeAxisBOnPlane X X HingeAxisA)*dot(hingeAxisBOnPlane X,constraintAxisAY),WB)+
            // dot(WA,dot(hingeAxisBOnPlane X,hingeAxisA)*(ConstraintAxisAY X HingeAxisBOnPlane X)-(HingeAxisA X HingeAxisBOnPlane X)*dot(hingeAxisBOnPlane X,constraintAxisAY)*denom
            // C‘=(点(wB,点(hingeAxisBOnPlane X,hingeAxisA)*(HingeAxisBOnPlane X X ConstraintAxisAY)-
            // 点(hingeAxisBOnPlane X,constraintAxisAY)*(HingeAxisBOnPlane X X HingeAxisA)+
            // 点(WA,点(hingeAxisBOnPlane X,hingeAxisA)*(ConstraintAxisAY X HingeAxisBOnPlane X)-
            // 点(hingeAxisBOnPlane X,constraintAxisAY)*(HingeAxisA X HingeAxisBOnPlane X))*Denom
            // 请注意,Jacobian A的两个贡献矢量constraintAxisAY x hingeAxisBOnPlane X和hingeAxisA x hingeAxisBOnPlane X都与constraintAxisAX对齐。
            // 唯一剩下的问题是规模。通过使用ConstraintAxisAX打点来测量它。
            // (为简明起见,此处切换表示法：a=hingeAxisA,b=hingeAxisBOnPlane X,x=constraintAxisAX,y=constraintAxisAY)
            // (点(b,a)*交叉(y,b)-点(b,y)*交叉(a,b))/(点(b,a)^2+点(b,y)^2)
            // 比例=点((点(b,a)*交叉(y,b)-点(b,y)*交叉(a,b))/(点(b,a)^2+点(b,y)^2),x)
            // 比例=(点(b,a)*点(x,交叉(y,b))-点(b,y)*点(x,交叉(a,b)/(点(b,a)^2+点(b,y)^2)
            // 比例=(点(b,a)*点(b,交叉(x,y))-点(b,y)*点(b,交叉(x,a)/(点(b,a)^2+点(b,y)^2)
            // 比例=(点(b,a)*点(b,a)-点(b,y)*点(b,-y))/(点(b,a)^2+点(b,y)^2)
            // 比例=(点(b,a)*点(b,a)+点(b,y)*点(b,y))/(点(b,a)^2+点(b,y)^2)
            // 比例=1
            // 真方便啊！
            // JacobianA=[ConstraintAxisAX]
            // [ConstraintAxisAY]
            // JacobianB=-JacobianA

            // 注意,JA=-JB,但为了计算有效质量,符号是无关的。
            // 这使用通常的(J*M^-1*JT)^-1公式来计算有效质量,但是我们实际上利用了中间结果J*M^-1,所以我们直接计算它。
            Symmetric3x3Wide.MultiplyWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out projection.ImpulseToVelocityA);
            // 注意,我们在这里没有使用-JacobianA,所以我们实际上存储了转换的否定版本。这很好;我们只需在迭代中减去即可。
            Symmetric3x3Wide.MultiplyWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out projection.NegatedImpulseToVelocityB);
            Symmetric2x2Wide.CompleteMatrixSandwich(projection.ImpulseToVelocityA, jacobianA, out var angularA);
            Symmetric2x2Wide.CompleteMatrixSandwich(projection.NegatedImpulseToVelocityB, jacobianA, out var angularB);
            Symmetric2x2Wide.Add(angularA, angularB, out var inverseEffectiveMass);
            Symmetric2x2Wide.InvertWithoutOverlap(inverseEffectiveMass, out var effectiveMass);

            SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);
            Symmetric2x2Wide.Scale(effectiveMass, effectiveMassCFMScale, out effectiveMass);
            Symmetric2x2Wide.MultiplyTransposed(jacobianA, effectiveMass, out projection.VelocityToImpulseA);
            GetErrorAngles(hingeAxisA, hingeAxisB, jacobianA, out var errorAngle);
            // 注意否定：我们想要反对分离。TODO：可以说,应该将否定烘焙到PositionErrorToVelocity(给定其名称)。
            Vector2Wide.Scale(errorAngle, -positionErrorToVelocity, out var biasVelocity);
            Symmetric2x2Wide.TransformWithoutOverlap(biasVelocity, effectiveMass, out projection.BiasImpulse);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB, ref AngularHingeProjection projection, ref Vector2Wide csi)
        {
            Matrix2x3Wide.Transform(csi, projection.ImpulseToVelocityA, out var velocityChangeA);
            Vector3Wide.Add(angularVelocityA, velocityChangeA, out angularVelocityA);
            Matrix2x3Wide.Transform(csi, projection.NegatedImpulseToVelocityB, out var negatedVelocityChangeB);
            Vector3Wide.Subtract(angularVelocityB, negatedVelocityChangeB, out angularVelocityB);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularHingeProjection projection, ref Vector2Wide accumulatedImpulse)
        {
            ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref accumulatedImpulse);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularHingeProjection projection, ref Vector2Wide accumulatedImpulse)
        {
            // JB=-JA。这是(angularVelocityA*JA+angularVelocityB*JB)*EffectiveMass=>(angularVelocityA-angularVelocityB)*(JA*EffectiveMass)
            Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var difference);
            Matrix2x3Wide.TransformByTransposeWithoutOverlap(difference, projection.VelocityToImpulseA, out var csi);
            // CSI=投射.BiasImPulse-累加脉冲*投射.SoftnessImPulseScale-(csia线性+csiaAngular+csib线性+csibAngular);
            Vector2Wide.Scale(accumulatedImpulse, projection.SoftnessImpulseScale, out var softnessContribution);
            Vector2Wide.Add(softnessContribution, csi, out csi);
            Vector2Wide.Subtract(projection.BiasImpulse, csi, out csi);

            Vector2Wide.Add(accumulatedImpulse, csi, out accumulatedImpulse);

            ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi);
        }

    }

    public class AngularHingeTypeProcessor : TwoBodyTypeProcessor<AngularHingePrestepData, AngularHingeProjection, Vector2Wide, AngularHingeFunctions>
    {
        public const int BatchTypeId = 23;
    }
}

